#pragma once
#ifndef _UTILITY_LIDAR_ODOMETRY_H_
#define _UTILITY_LIDAR_ODOMETRY_H_

#include "third_party/include/simple_time.h" // timer

#include <pcl/kdtree/kdtree_flann.h>  // pcl include kdtree_flann throws error if PCL_NO_PRECOMPILE

#include <iostream>
#include <nav_msgs/msg/odometry.hpp>
#include <nav_msgs/msg/path.hpp>
#include <opencv2/opencv.hpp>
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/imu.hpp>
#include <sensor_msgs/msg/nav_sat_fix.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <std_msgs/msg/float64_multi_array.hpp>
#include <std_msgs/msg/header.hpp>
#include <std_msgs/msg/string.hpp>
#include <visualization_msgs/msg/marker.hpp>
#include <visualization_msgs/msg/marker_array.hpp>
// is defined before
#define PCL_NO_PRECOMPILE
#include <pcl/common/common.h>
#include <pcl/common/transforms.h>
#include <pcl/filters/crop_box.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/range_image/range_image.h>
#include <pcl/registration/icp.h>
#include <pcl_conversions/pcl_conversions.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_eigen/tf2_eigen.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_ros/transform_listener.h>

#include <algorithm>
#include <array>
#include <cfloat>
#include <cmath>
#include <ctime>
#include <deque>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <iterator>
#include <limits>
#include <mutex>
#include <pcl/search/impl/search.hpp>
#include <queue>
#include <sstream>
#include <string>
#include <thread>
#include <vector>

using namespace std;

typedef pcl::PointXYZI PointType;

enum class SensorType {
    VELODYNE,
    OUSTER,
    LIVOX,
    LIVOX_MID360
};

class ParamServer : public rclcpp::Node {
public:
    std::string robot_id_;

    // Topics
    string point_cloud_topic_;
    string imu_topic_;
    string odom_topic_;
    string gps_topic_;

    // Frames
    string lidar_frame_;
    string base_link_frame_;
    string odometry_frame_;
    string map_frame_;

    // GPS Settings
    bool  use_imu_heading_initialization_;
    bool  use_gps_elevation_;
    float gps_cov_threshold_;
    float pose_cov_threshold_;

    // Save pcd
    bool   save_pcd_;
    string save_pcd_directory_;

    // Lidar Sensor Configuration
    SensorType sensor = SensorType::OUSTER;
    int        N_SCAN;
    int        Horizon_SCAN;
    int        down_sample_rate_;
    float      lidar_min_range_;
    float      lidar_max_range_;

    // IMU
    float              imu_acc_noise_;
    float              imu_gyr_noise_;
    float              imu_acc_bias_n_;
    float              imu_gyr_bias_n_;
    float              imu_gravity_;
    float              imu_rpy_weight_;
    vector<double>     ext_rot_vec_;
    vector<double>     ext_rpy_vec_;
    vector<double>     ext_trans_vec_;
    Eigen::Matrix3d    ext_rot_;
    Eigen::Matrix3d    ext_rpy_;
    Eigen::Vector3d    ext_trans_;
    Eigen::Quaterniond ext_q_rpy_;

    // LOAM
    float edge_threshold_;
    float surf_threshold_;
    int   edge_feature_min_valid_num_;
    int   surf_feature_min_valid_num_;

    // voxel filter paprams
    float odometry_surf_leaf_size_;
    float mapping_corner_leaf_size_;
    float mapping_surf_leaf_size_;

    float z_tollerance_;
    float rotation_tollerance_;

    // CPU Params
    int    number_of_cores_;
    double mapping_process_interval_;

    // Surrounding map
    float surrounding_key_frame_adding_dist_threshold_;
    float surrounding_key_frame_adding_angle_threshold_;
    float surrounding_key_frame_density_;
    float surrounding_key_frame_search_radius_;

    // Loop closure
    bool  loop_closure_enable_flag_;
    float loop_closure_frequency_;
    int   surrounding_key_frame_size_;
    float history_key_frame_search_radius_;
    float history_key_frame_search_time_off_;
    int   history_key_frame_search_num_;
    float history_key_frame_fitness_score_;

    // global map visualization radius
    float global_map_visualization_search_radius_;
    float global_map_visualization_pose_density_;
    float global_map_visualization_leaf_size_;

    ParamServer(std::string node_name, const rclcpp::NodeOptions &options) : Node(node_name, options) {
        declare_parameter("pointCloudTopic", "points");
        get_parameter("pointCloudTopic", point_cloud_topic_);
        declare_parameter("imuTopic", "imu/data");
        get_parameter("imuTopic", imu_topic_);
        declare_parameter("odomTopic", "lio_sam/odometry/imu");
        get_parameter("odomTopic", odom_topic_);
        declare_parameter("gpsTopic", "lio_sam/odometry/gps");
        get_parameter("gpsTopic", gps_topic_);

        declare_parameter("lidarFrame", "laser_data_frame");
        get_parameter("lidarFrame", lidar_frame_);
        declare_parameter("baselinkFrame", "base_link");
        get_parameter("baselinkFrame", base_link_frame_);
        declare_parameter("odometryFrame", "odom");
        get_parameter("odometryFrame", odometry_frame_);
        declare_parameter("mapFrame", "map");
        get_parameter("mapFrame", map_frame_);

        declare_parameter("useImuHeadingInitialization", false);
        get_parameter("useImuHeadingInitialization", use_imu_heading_initialization_);
        declare_parameter("useGpsElevation", false);
        get_parameter("useGpsElevation", use_gps_elevation_);
        declare_parameter("gpsCovThreshold", 2.0);
        get_parameter("gpsCovThreshold", gps_cov_threshold_);
        declare_parameter("poseCovThreshold", 25.0);
        get_parameter("poseCovThreshold", pose_cov_threshold_);

        declare_parameter("savePCD", false);
        get_parameter("savePCD", save_pcd_);
        declare_parameter("savePCDDirectory", "/Downloads/LOAM/");
        get_parameter("savePCDDirectory", save_pcd_directory_);

        std::string sensorStr;
        declare_parameter("sensor", "ouster");
        get_parameter("sensor", sensorStr);
        if (sensorStr == "velodyne") {
            sensor = SensorType::VELODYNE;
        } else if (sensorStr == "ouster") {
            sensor = SensorType::OUSTER;
        } else if (sensorStr == "livox") {
            sensor = SensorType::LIVOX;
        } else if (sensorStr == "livox_mid360") {
            sensor = SensorType::LIVOX_MID360;
        } else {
            RCLCPP_ERROR_STREAM(get_logger(),
                                "Invalid sensor type (must be either 'velodyne' or "
                                "'ouster' or 'livox'): "
                                    << sensorStr);
            rclcpp::shutdown();
        }

        declare_parameter("N_SCAN", 64);
        get_parameter("N_SCAN", N_SCAN);
        declare_parameter("Horizon_SCAN", 512);
        get_parameter("Horizon_SCAN", Horizon_SCAN);
        declare_parameter("downsampleRate", 1);
        get_parameter("downsampleRate", down_sample_rate_);
        declare_parameter("lidarMinRange", 5.5);
        get_parameter("lidarMinRange", lidar_min_range_);
        declare_parameter("lidarMaxRange", 1000.0);
        get_parameter("lidarMaxRange", lidar_max_range_);

        declare_parameter("imuAccNoise", 9e-4);
        get_parameter("imuAccNoise", imu_acc_noise_);
        declare_parameter("imuGyrNoise", 1.6e-4);
        get_parameter("imuGyrNoise", imu_gyr_noise_);
        declare_parameter("imuAccBiasN", 5e-4);
        get_parameter("imuAccBiasN", imu_acc_bias_n_);
        declare_parameter("imuGyrBiasN", 7e-5);
        get_parameter("imuGyrBiasN", imu_gyr_bias_n_);
        declare_parameter("imuGravity", 9.80511);
        get_parameter("imuGravity", imu_gravity_);
        declare_parameter("imuRPYWeight", 0.01);
        get_parameter("imuRPYWeight", imu_rpy_weight_);

        double              ida[] = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0};
        std::vector<double> id(ida, std::end(ida));
        declare_parameter("extrinsicRot", id);
        get_parameter("extrinsicRot", ext_rot_vec_);
        declare_parameter("extrinsicRPY", id);
        get_parameter("extrinsicRPY", ext_rpy_vec_);
        double              zea[] = {0.0, 0.0, 0.0};
        std::vector<double> ze(zea, std::end(zea));
        declare_parameter("extrinsicTrans", ze);
        get_parameter("extrinsicTrans", ext_trans_vec_);

        ext_rot_   = Eigen::Map<const Eigen::Matrix<double, -1, -1, Eigen::RowMajor>>(ext_rot_vec_.data(), 3, 3);
        ext_rpy_   = Eigen::Map<const Eigen::Matrix<double, -1, -1, Eigen::RowMajor>>(ext_rpy_vec_.data(), 3, 3);
        ext_trans_ = Eigen::Map<const Eigen::Matrix<double, -1, -1, Eigen::RowMajor>>(ext_trans_vec_.data(), 3, 1);
        ext_q_rpy_ = Eigen::Quaterniond(ext_rpy_);

        declare_parameter("edgeThreshold", 1.0);
        get_parameter("edgeThreshold", edge_threshold_);
        declare_parameter("surfThreshold", 0.1);
        get_parameter("surfThreshold", surf_threshold_);
        declare_parameter("edgeFeatureMinValidNum", 10);
        get_parameter("edgeFeatureMinValidNum", edge_feature_min_valid_num_);
        declare_parameter("surfFeatureMinValidNum", 100);
        get_parameter("surfFeatureMinValidNum", surf_feature_min_valid_num_);

        declare_parameter("odometrySurfLeafSize", 0.4);
        get_parameter("odometrySurfLeafSize", odometry_surf_leaf_size_);
        declare_parameter("mappingCornerLeafSize", 0.2);
        get_parameter("mappingCornerLeafSize", mapping_corner_leaf_size_);
        declare_parameter("mappingSurfLeafSize", 0.4);
        get_parameter("mappingSurfLeafSize", mapping_surf_leaf_size_);

        declare_parameter("z_tollerance", 1000.0);
        get_parameter("z_tollerance", z_tollerance_);
        declare_parameter("rotation_tollerance", 1000.0);
        get_parameter("rotation_tollerance", rotation_tollerance_);

        declare_parameter("numberOfCores", 4);
        get_parameter("numberOfCores", number_of_cores_);
        declare_parameter("mappingProcessInterval", 0.15);
        get_parameter("mappingProcessInterval", mapping_process_interval_);

        declare_parameter("surroundingkeyframeAddingDistThreshold", 1.0);
        get_parameter("surroundingkeyframeAddingDistThreshold", surrounding_key_frame_adding_dist_threshold_);
        declare_parameter("surroundingkeyframeAddingAngleThreshold", 0.2);
        get_parameter("surroundingkeyframeAddingAngleThreshold", surrounding_key_frame_adding_angle_threshold_);
        declare_parameter("surroundingKeyframeDensity", 2.0);
        get_parameter("surroundingKeyframeDensity", surrounding_key_frame_density_);
        declare_parameter("surroundingKeyframeSearchRadius", 50.0);
        get_parameter("surroundingKeyframeSearchRadius", surrounding_key_frame_search_radius_);

        declare_parameter("loopClosureEnableFlag", true);
        get_parameter("loopClosureEnableFlag", loop_closure_enable_flag_);
        declare_parameter("loopClosureFrequency", 1.0);
        get_parameter("loopClosureFrequency", loop_closure_frequency_);
        declare_parameter("surroundingKeyframeSize", 50);
        get_parameter("surroundingKeyframeSize", surrounding_key_frame_size_);
        declare_parameter("historyKeyframeSearchRadius", 15.0);
        get_parameter("historyKeyframeSearchRadius", history_key_frame_search_radius_);
        declare_parameter("historyKeyframeSearchTimeDiff", 30.0);
        get_parameter("historyKeyframeSearchTimeDiff", history_key_frame_search_time_off_);
        declare_parameter("historyKeyframeSearchNum", 25);
        get_parameter("historyKeyframeSearchNum", history_key_frame_search_num_);
        declare_parameter("historyKeyframeFitnessScore", 0.3);
        get_parameter("historyKeyframeFitnessScore", history_key_frame_fitness_score_);

        declare_parameter("globalMapVisualizationSearchRadius", 1000.0);
        get_parameter("globalMapVisualizationSearchRadius", global_map_visualization_search_radius_);
        declare_parameter("globalMapVisualizationPoseDensity", 10.0);
        get_parameter("globalMapVisualizationPoseDensity", global_map_visualization_pose_density_);
        declare_parameter("globalMapVisualizationLeafSize", 1.0);
        get_parameter("globalMapVisualizationLeafSize", global_map_visualization_leaf_size_);

        usleep(100);
    }

    sensor_msgs::msg::Imu imuConverter(const sensor_msgs::msg::Imu &imu_in) {
        sensor_msgs::msg::Imu imu_out = imu_in;
        // rotate acceleration
        Eigen::Vector3d acc(imu_in.linear_acceleration.x, imu_in.linear_acceleration.y, imu_in.linear_acceleration.z);

        // mid360加速度单位是g, 需要转换成m/s2
        if (sensor == SensorType::LIVOX_MID360) {
            acc *= imu_gravity_;
        }

        acc                           = ext_rot_ * acc;
        imu_out.linear_acceleration.x = acc.x();
        imu_out.linear_acceleration.y = acc.y();
        imu_out.linear_acceleration.z = acc.z();
        // rotate gyroscope
        Eigen::Vector3d gyr(imu_in.angular_velocity.x, imu_in.angular_velocity.y, imu_in.angular_velocity.z);
        gyr                        = ext_rot_ * gyr;
        imu_out.angular_velocity.x = gyr.x();
        imu_out.angular_velocity.y = gyr.y();
        imu_out.angular_velocity.z = gyr.z();
        // rotate roll pitch yaw
        Eigen::Quaterniond q_from(imu_in.orientation.w, imu_in.orientation.x, imu_in.orientation.y,
                                  imu_in.orientation.z);
        Eigen::Quaterniond q_final = q_from * ext_q_rpy_;
        imu_out.orientation.x      = q_final.x();
        imu_out.orientation.y      = q_final.y();
        imu_out.orientation.z      = q_final.z();
        imu_out.orientation.w      = q_final.w();

        if (sqrt(q_final.x() * q_final.x() + q_final.y() * q_final.y() + q_final.z() * q_final.z() +
                 q_final.w() * q_final.w()) < 0.1) {
            RCLCPP_ERROR(get_logger(), "Invalid quaternion, please use a 9-axis IMU!");
            rclcpp::shutdown();
        }

        return imu_out;
    }
};

sensor_msgs::msg::PointCloud2 publishCloud(rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr thisPub,
                                           pcl::PointCloud<PointType>::Ptr thisCloud, rclcpp::Time thisStamp,
                                           std::string thisFrame) {
    sensor_msgs::msg::PointCloud2 tempCloud;
    pcl::toROSMsg(*thisCloud, tempCloud);
    tempCloud.header.stamp    = thisStamp;
    tempCloud.header.frame_id = thisFrame;
    if (thisPub->get_subscription_count() != 0)
        thisPub->publish(tempCloud);
    return tempCloud;
}

template <typename T>
double stamp2Sec(const T &stamp) {
    return rclcpp::Time(stamp).seconds();
}

template <typename T>
void imuAngular2rosAngular(sensor_msgs::msg::Imu *thisImuMsg, T *angular_x, T *angular_y, T *angular_z) {
    *angular_x = thisImuMsg->angular_velocity.x;
    *angular_y = thisImuMsg->angular_velocity.y;
    *angular_z = thisImuMsg->angular_velocity.z;
}

template <typename T>
void imuAccel2rosAccel(sensor_msgs::msg::Imu *thisImuMsg, T *acc_x, T *acc_y, T *acc_z) {
    *acc_x = thisImuMsg->linear_acceleration.x;
    *acc_y = thisImuMsg->linear_acceleration.y;
    *acc_z = thisImuMsg->linear_acceleration.z;
}

template <typename T>
void imuRPY2rosRPY(sensor_msgs::msg::Imu *thisImuMsg, T *rosRoll, T *rosPitch, T *rosYaw) {
    double          imuRoll, imuPitch, imuYaw;
    tf2::Quaternion orientation;
    tf2::fromMsg(thisImuMsg->orientation, orientation);
    tf2::Matrix3x3(orientation).getRPY(imuRoll, imuPitch, imuYaw);

    *rosRoll  = imuRoll;
    *rosPitch = imuPitch;
    *rosYaw   = imuYaw;
}

float pointDistance(PointType p) {
    return sqrt(p.x * p.x + p.y * p.y + p.z * p.z);
}

float pointDistance(PointType p1, PointType p2) {
    return sqrt((p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y) + (p1.z - p2.z) * (p1.z - p2.z));
}

rmw_qos_profile_t qos_profile{RMW_QOS_POLICY_HISTORY_KEEP_LAST,
                              1,
                              RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT,
                              RMW_QOS_POLICY_DURABILITY_VOLATILE,
                              RMW_QOS_DEADLINE_DEFAULT,
                              RMW_QOS_LIFESPAN_DEFAULT,
                              RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT,
                              RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT,
                              false};

auto qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, qos_profile.depth), qos_profile);

rmw_qos_profile_t qos_profile_imu{RMW_QOS_POLICY_HISTORY_KEEP_LAST,
                                  2000,
                                  RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT,
                                  RMW_QOS_POLICY_DURABILITY_VOLATILE,
                                  RMW_QOS_DEADLINE_DEFAULT,
                                  RMW_QOS_LIFESPAN_DEFAULT,
                                  RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT,
                                  RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT,
                                  false};

auto qos_imu = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile_imu.history, qos_profile_imu.depth), qos_profile_imu);

rmw_qos_profile_t qos_profile_lidar{RMW_QOS_POLICY_HISTORY_KEEP_LAST,
                                    5,
                                    RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT,
                                    RMW_QOS_POLICY_DURABILITY_VOLATILE,
                                    RMW_QOS_DEADLINE_DEFAULT,
                                    RMW_QOS_LIFESPAN_DEFAULT,
                                    RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT,
                                    RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT,
                                    false};

auto qos_lidar =
    rclcpp::QoS(rclcpp::QoSInitialization(qos_profile_lidar.history, qos_profile_lidar.depth), qos_profile_lidar);

#endif
